﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using System.IO;

namespace MapViewer
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {

        double zoom = 1f;
        List<Node> allNodes = new List<Node>();
        Dictionary<string, Node> indexes = new Dictionary<string, Node>();
        Dictionary<string, osmNode> osmNodes = new Dictionary<string, osmNode>();
        FileStream fs = new FileStream("facts.pddl", FileMode.Create);
        StreamWriter wr;
        

        List<String> Objects = new List<string>();
        String Facts = "";
        String startPoint = "69097200";
        List<string> deliveryPoints = new List<string>();
        int numTrucks = 3;

        Random rand = new Random(1);

        public MainWindow()
        {
            InitializeComponent();
            wr = new StreamWriter(fs);

            #region Loading
            System.Xml.Serialization.XmlSerializer ser = new System.Xml.Serialization.XmlSerializer(typeof(osm));
            osm root = null;
            using (System.Xml.XmlReader reader = System.Xml.XmlReader.Create("map.xml"))
            {
                root = (osm)ser.Deserialize(reader);
            }

            Dictionary<string, osmNode> map = new Dictionary<string, osmNode>();

            //Find the dimensions to scale by
            double xMin = float.MaxValue;
            double xMax = float.MinValue;
            double yMin = float.MaxValue;
            double yMax = float.MinValue;
            foreach (osmNode node in root.node)
            {
                double lat = double.Parse(node.lat);
                double lon = double.Parse(node.lon);

                if (lat < xMin) xMin = lat;
                if (lat > xMax) xMax = lat;
                if (lon < yMin) yMin = lon;
                if (lon > yMax) yMax = lon;

                map.Add(node.id, node);
                Node n = new Node();
                n.sid = node.id;
                n.id = int.Parse(n.sid);
                allNodes.Add(n);
                indexes.Add(node.id, n);

                osmNodes.Add(node.id, node);
            }

            /*Line l = new Line();
            l.X1 = 1;
            l.Y1 = 1;
            l.X2 = canvas.Width - 1;
            l.Y2 = canvas.Height - 1;
            l.Stroke = System.Windows.Media.Brushes.LightSteelBlue;
            l.StrokeThickness = 2;
            canvas.Children.Add(l);*/
            SolidColorBrush brush = new SolidColorBrush(Color.FromRgb(255, 255, 100));

            int counter = 0;
            List<string> AllNodes = new List<string>();
            List<string> Intersections = new List<string>();

            //find intersections
            foreach (osmWay way in root.way)
            {
                foreach (osmWayND node in way.nd)
                {
                    if (AllNodes.Contains(node.@ref))
                    {
                        Intersections.Add(node.@ref);
                        if (rand.NextDouble() < .004) deliveryPoints.Add(node.@ref);
                    }
                    AllNodes.Add(node.@ref);
                }
            }
            #endregion

            #region draw roads
            foreach (osmWay way in root.way)
            {
                osmNode lastNode = null;
                bool isRoad = false;
                if(way.tag != null)
                foreach (tag t in way.tag)
                {
                    if (t.k.CompareTo("highway") == 0)
                    {
                        isRoad = true;
                        break;
                    }
                }
                if (!isRoad)
                {
                    //Remove the non-road intersections
                    foreach (osmWayND nodRef in way.nd)
                        Intersections.Remove(nodRef.@ref);
                    continue;
                }

                foreach (osmWayND nodeRef in way.nd)
                {
                    if (Intersections.Contains(nodeRef.@ref))
                    {
                        osmNode node = map[nodeRef.@ref];
                        if (lastNode == null)
                        {
                            lastNode = node;
                            continue;
                        }

                        Node lNode = indexes[lastNode.id];
                        lNode.connected.Add(indexes[node.id]);

                        Node cNode = indexes[node.id];
                        cNode.connected.Add(indexes[lastNode.id]);

                        double[] coords = getRatio(double.Parse(lastNode.lat), double.Parse(lastNode.lon), xMin, xMax, yMin, yMax);
                        double[] coords2 = getRatio(double.Parse(node.lat), double.Parse(node.lon), xMin, xMax, yMin, yMax);
                        double posX = canvas.Width * coords[0];
                        double posY = canvas.Height * coords[1];
                        double posX2 = canvas.Width * coords2[0];
                        double posY2 = canvas.Height * coords2[1];
                        lastNode = node;

                        Line l = new Line();
                        l.X1 = posX;
                        l.Y1 = posY;
                        l.X2 = posX2;
                        l.Y2 = posY2;
                        l.Stroke = System.Windows.Media.Brushes.LightSteelBlue;
                        l.StrokeThickness = .2;

                        canvas.Children.Add(l);
                    }
                }
            }
            #endregion

            List<string> invalidPoints = new List<string>();
            int maxDistance = 0;
            for (int k = -1; k < deliveryPoints.Count; k++)
            {
                Console.WriteLine(k + " / " + deliveryPoints.Count);
                for (int j = 0; j < deliveryPoints.Count; j++)
                {
                    if (k == j) continue;

                    Node src = k < 0 ? indexes[startPoint] : indexes[deliveryPoints[k]];
                    Node dest = indexes[deliveryPoints[j]];
                    List<Node> path = Search.Astar(src, dest, allNodes);
                    if (path == null)
                    {
                        if(k == -1)
                            invalidPoints.Add(deliveryPoints[j]);//This point can't be reached from the start
                        continue;//No path found
                    }
                    path.Add(dest);

                    float distance = 0;
                    for (int i = 1; i < path.Count; i++)
                    {
                        distance++;
                        osmNode lastNode = osmNodes[path[i - 1].sid];
                        osmNode node = osmNodes[path[i].sid];


                        double[] coords = getRatio(double.Parse(lastNode.lat), double.Parse(lastNode.lon), xMin, xMax, yMin, yMax);
                        double[] coords2 = getRatio(double.Parse(node.lat), double.Parse(node.lon), xMin, xMax, yMin, yMax);
                        double posX = canvas.Width * coords[0];
                        double posY = canvas.Height * coords[1];
                        double posX2 = canvas.Width * coords2[0];
                        double posY2 = canvas.Height * coords2[1];

                        Line l = new Line();
                        l.X1 = posX;
                        l.Y1 = posY;
                        l.X2 = posX2;
                        l.Y2 = posY2;
                        l.Stroke = System.Windows.Media.Brushes.Red;
                        l.StrokeThickness = .2;

                        canvas.Children.Add(l);

                        //Keep track of the objects PDDL needs
                        if (!Objects.Contains(lastNode.id))
                            Objects.Add(lastNode.id);
                        if (!Objects.Contains(node.id))
                            Objects.Add(node.id);

                        //Facts += "(Connected L" + lastNode.id + " L" + node.id + ")\n";
                    }
                    Facts += "(Connected L" + path[0].id + " L" + path[path.Count - 1].id + " D" + (int)distance + ")\n";
                    if (distance > maxDistance) maxDistance = (int)distance;
                }
            }
            foreach (string str in invalidPoints)
                deliveryPoints.Remove(str);

            //Write to file
            wr.WriteLine("(define (problem routing-gpln)" +
                    "(:domain routing-gpln)" +
                    "(:objects");
            foreach (string str in Objects) wr.Write("L" + str + " ");//Add nodes
            for (int i = 0; i <= maxDistance; i++ ) wr.Write("D" + i + " ");//Add distances
            for (int i = 1; i <= numTrucks; i++) wr.Write("T" + i + " ");//Add trucks
            wr.WriteLine("\n)\n(:init\n");
            for (int i = 1; i <= numTrucks; i++) wr.Write("(Truck T" + i + ")(ats T" + i + " L" + startPoint + ")(Dist T" + i + " D0)");
            wr.WriteLine(Facts);
            for (int i = 0; i < maxDistance; i++) wr.WriteLine("(Decr D" + (i + 1) + " D" + (i) + ")");
            wr.Write(")\n(:goal ");
            foreach (string str in deliveryPoints) wr.Write("(Delivered L" + str + ") ");
            wr.Write(")\n)");
            wr.Close();

            //Draw intersection points
            foreach (osmNode node in root.node)
            {
                //if (counter++ > 5000) break;
                if (!Intersections.Contains(node.id)) continue;
                Ellipse nodeGfx = new Ellipse();
                double[] coords = getRatio(double.Parse(node.lat), double.Parse(node.lon), xMin, xMax, yMin, yMax);

                nodeGfx.Width = .5;
                nodeGfx.Height = .5;
                nodeGfx.Fill = brush;
                nodeGfx.StrokeThickness = .01;
                nodeGfx.Stroke = Brushes.Black;

                double posX = canvas.Width * coords[0] - nodeGfx.Width / 2;
                double posY = canvas.Height * coords[1] - nodeGfx.Height / 2;
                nodeGfx.SetValue(Canvas.LeftProperty, posX);
                nodeGfx.SetValue(Canvas.TopProperty, posY);

                canvas.Children.Add(nodeGfx);
            }
        }

        public double[] getRatio(double lat, double lon, double latMin, double latMax, double lonMin, double lonMax)
        {
            double[] retn = new double[2];
            retn[0] = (lat - latMin) / (latMax - latMin);
            retn[1] = (lon - lonMin) / (lonMax - lonMin);

            return retn;
        }

        private void canvas_MouseWheel(object sender, MouseWheelEventArgs e)
        {

            double zoomInc = e.Delta > 0 ? .05f : -.05f;
            zoom += zoomInc;

            TransformGroup group = new TransformGroup();
            group.Children.Add(new TranslateTransform(offset.X, offset.Y));
            group.Children.Add(new ScaleTransform(zoom, zoom));
            canvas.RenderTransform = group;
        }

        private void canvas_MouseLeftButtonDown(object sender, MouseButtonEventArgs e)
        {
            lastMouse = e.GetPosition(canvas);
            canvas.CaptureMouse();
        }

        private void canvas_MouseLeftButtonUp(object sender, MouseButtonEventArgs e)
        {
            canvas.ReleaseMouseCapture();
        }

        Point lastMouse;
        Point offset;
        private void canvas_MouseMove(object sender, MouseEventArgs e)
        {
            if (canvas.IsMouseCaptured)
            {
                Point current = e.GetPosition(canvas);
                Point delta = new Point();
                delta.X = current.X - lastMouse.X;
                delta.Y = current.Y - lastMouse.Y;
                offset.X = delta.X;
                offset.Y = delta.Y;

                TransformGroup group = new TransformGroup();
                group.Children.Add(new TranslateTransform(offset.X, offset.Y));
                group.Children.Add(new ScaleTransform(zoom, zoom));
                canvas.RenderTransform = group;
            }
        }
    }
}
